import sys
import time
import threading

try :
    import serial
except ImportError :
    pass


CONVERT_CRLF = 2
CONVERT_CR   = 1
CONVERT_LF   = 0

port  = 2
baudrate = 115000
echo = 0
convert_outgoing = CONVERT_CRLF
rtscts = 0
xonxoff = 0
repr_mode = 0


class SCC32_SIMULATOR( threading.Thread ) :
    def __init__(self ) :
        threading.Thread.__init__(self, name = "SCC32_SIMULATOR" )
        self.daemon = True
        self.go_event = threading.Event()
        self.stopped = threading.Event()
        self.pos = [1500] * 32
        self.targets = [1500] * 32
        self.start_pos = [1500] * 32
        self.actions = [None] * 32
        self.stopped.set()

    def set_action( self, servo, action ) :
        self.actions[servo] = action
        
    def time(self) :
        return int(time.time() * 1000)

    def wait_stopped(self) :
        self.stopped.wait()
        
    def goto( self, tgts, delay ) :
        self.wait_stopped()
        self.stopped.clear()

        self.start_date = self.time()
        self.target_date = self.start_date + delay
        for servo, pos in tgts.items() :
            self.targets[servo] = pos
            self.start_pos[servo] = self.pos[servo]
        self.go_event.set()
        
    def run(self) :
        while True :
            self.go_event.wait()
            self.go_event.clear()
            while True :
                t = self.time()
                move = []
                if t >= self.target_date :
                    for servo in range(32) :
                        if self.pos[servo] != self.targets[servo] :
                           self.pos[servo] = self.targets[servo]
                           self.move( servo )
                    break
                else :
                    v1 = (self.target_date - t) / (self.target_date - self.start_date)
                    
                    for servo in range(32) :
                        if self.pos[servo] != self.targets[servo] :
                            
                            self.pos[servo] = v1 * self.start_pos[servo] + (1-v1) * self.targets[servo]
                            self.move( servo )
                time.sleep(0.1)
            self.stopped.set()
            
    def move(self, servo) :
        if self.actions[servo] :
            self.actions[servo]( self.pos[servo] )
                
class SCC32 :
    def __init__(self, port, baudrate = baudrate , rtscts=rtscts, xonxoff=xonxoff, timeout = None, simulation = False) :
        self._simulation = simulation
        if self._simulation : 
            self._s = sys.stdout
            self.simulator = SCC32_SIMULATOR()
            self.simulator.start()
        else :
            self._s = serial.Serial(port, baudrate, rtscts=rtscts, xonxoff=xonxoff, timeout = timeout)
        self._pos = [ None ] * 32
        
    def close(self) :
        if self._simulation :
            del self.simulator
        else :
            self._s.close()

    def wait_stop(self) :
        if self._simulation :
            self.simulator.wait_stopped()
        else :
            while True :
                self._s.write("Q\r")
                res = self._s.read()
                print(res, end=' ')
                if res == "." :
                    break
                time.sleep(0.1)

    def send( self, positions, delay = None, speeds={} ) :
        self.wait_stop()

        command=""
        for i in range(32) :
            if i not in positions :
                continue

            command += "#%d" % i
            
            command += " P%d" % positions[i]

            if i in speeds :
                command += " S%d" % speeds[i]

        if delay :
            command += " T%d" % delay

        print("COMMAND = %s" % command)

        if self._simulation :
            self.simulator.goto( positions, delay )

        else :
            self._s.write( command +'\r' )


    def send1( self, servo, position, delay ) :
        self.send( { servo : position }, delay )

    def get_position( self, *servos ) :
        if self._simulation : 
            return self.simulator.pos
    
        self.wait_stop()

        command = ""
        for i in servos :
            command += "Q%d " % i

        print("COMMAND = %s" % command)
        self._s.write( command +'\r' )

        values = []
        for i in servos :
            val = ord( self._s.read() )
            print(i, val)
            if val <= 0 :
                values.append(None)
            else :
                values.append(val*10)

        return values

    def set_action(self, servo, action) :
        if self._simulation :
            self.simulator.set_action( servo, action )
            
if __name__ == "__main__" :
    s = SCC32( port, simulation = True )
    import varm
    a = varm.Arm()

    s.set_action( 2, a.cshoulder )
    s.set_action( 0, a.cbase )
    
    s.send( {0:1500, 1:1500, 2:1500, 
             3:1500, 4:1500, 5:1500,
             6:1500, 7:1500} , 
           1000)
    
    
